
#include "FreeRTOS_speed.h"


void speed(int left_speed, int right_speed){
	/*
	if(left_speed > 255)					
		left_speed = 255;	
	else if(left_speed < -255)
		left_speed = -255;	

	if(right_speed > 255){					
		right_speed = 255;
		
	}		
	else if(right_speed < -255){
		right_speed = -255;	
	}
	*/
	/* DC left */
	if (left_speed >= 0)	{
		LEFT_SPEED = left_speed;
		outdir |= DC_LEFT_FORWARD;
	}
	else {
		left_speed= -left_speed;
		LEFT_SPEED = left_speed; 
		outdir &= DC_LEFT_BACK; 
	}

	/* DC right */
	if (right_speed >= 0)
	{
		PORTD = right_speed & 0xff;
		RIGHT_SPEED = right_speed; 
		outdir |= DC_RIGHT_FORWARD;//set bit DO54 DIR2
		//RIGHT_DIR = TIEN;
		
	}
	else
	{
		PORTD = right_speed & 0xff;
		right_speed= -right_speed;
		
		RIGHT_SPEED = right_speed;
		outdir &=  DC_RIGHT_BACK; //clear bit DO55 DIR3
		//RIGHT_DIR = LUI;
	}
	writeIO(OUTDIR,outdir);//true set or clear dir bit	
}

